
/**********************************************************************\
*																																	  	 *
* PROGRAM: Arm Servo Driver    	      															  *
* VERSION: 1.2																										    *
* PURPOSE: This program uses a potentiometer to measure the height    *
*          of an arm, and runs the motor to a given height            *
* AUTHOR: Samuel Rosenstein                                           *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/


#include "drivers/common.h"
#include "drivers/HTPB-driver.h"

#define leftB 2
#define rightB 1
#define MIN_HEIGHT_ARM 119


void _allignHeight(int height) {
  int current;
  current = HTPBreadADC(HTPB, 4, 10);

  height = max(height, MIN_HEIGHT_ARM);

  while (abs(height - current) > 10) {
    current = HTPBreadADC(HTPB, 4, 10);
    if ((current - height) > 0) {
      motor[f] = 15;
    }
    if ((current - height) < 0) {
      motor[f] = -9;
    }
    nxtDisplayTextLine(2, "current: %4d", current);
  }
  motor[f] = 0;
}

void allignHeight(int height) {
  int current;
  current = HTPBreadADC(HTPB, 4, 10);
  if ((current - height) > 0) {
     _allignHeight(height - 50);
     wait1Msec(100);
  }
  _allignHeight(height);
}


task main() {
  int height = HTPBreadADC(HTPB, 4, 10);
  int current = HTPBreadADC(HTPB, 4, 10);
  int speed;
  nMotorPIDSpeedCtrl[f] = mtrSpeedReg;
  wait10Msec(60);
  while (nNxtButtonPressed != 3) {
    nxtDisplayTextLine(3, "height: %4d", height);
    if (nNxtButtonPressed == leftB) {
      height = height + 10;
      wait10Msec(20);
    }
    if (nNxtButtonPressed == rightB) {
      height = height - 10;
      wait10Msec(20);
    }
    int current;
    current = HTPBreadADC(HTPB, 4, 10);
    nxtDisplayTextLine(2, "current: %4d", current);
    nxtDisplayTextLine(3, "height: %4d", height);
  }
  allignHeight(height);
  while(true) {
  current = HTPBreadADC(HTPB, 4, 10);
  nxtDisplayTextLine(2, "current: %4d", current);
}
}
